package org.electrode.system;

import org.electrode.base.XboxJoystick;
import org.electrode.common.NetworkTableVariable;
import org.electrode.common.RobotSystem;
import org.electrode.common.SimpleVariable;
import org.electrode.common.Variable;
import org.electrode.task.Begin;
import org.electrode.task.PeriodicTask;
import org.electrode.task.TaskWorker;
import org.electrode.task.WaitTask;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.tables.ITable;

public class Shooter extends RobotSystem {
	private final XboxJoystick control;
	private final long SECOND = 1000;
	private Victor front;
	private Victor rear;
	private Servo feeder;
	// private Victor tilt;
	private DigitalInput upperLimit;
	private DigitalInput lowerLimit;
	// private AnalogChannel tiltPot;
	private NetworkTableVariable onSpeed;
	private NetworkTableVariable offSpeed;
	private NetworkTableVariable reverseSpeed;
	private NetworkTableVariable feederFinalPosition;
	private NetworkTableVariable feederInitialPosition;
	private TaskWorker autoWorker;

	public Shooter(XboxJoystick xbox) {
		this.control = xbox;
	}

	public void init() {
		front = new Victor(5);
		rear = new Victor(6);
		// tilt = new Victor(7);
		feeder = new Servo(8);
		upperLimit = new DigitalInput(5);
		lowerLimit = new DigitalInput(6);
		// tiltPot = new AnalogChannel(1);

		onSpeed = new NetworkTableVariable("ShooterOnSpeed", -1);
		offSpeed = new NetworkTableVariable("ShooterOffSpeed", 0);
		reverseSpeed = new NetworkTableVariable("ShooterReverseSpeed", 0.25);
		feederFinalPosition = new NetworkTableVariable("FeederFinalPosition",
				0.25);
		feederInitialPosition = new NetworkTableVariable(
				"FeederInitialPosition", 1);

		ITable table = NetworkTable.getTable(getName());
		front.initTable(table.getSubTable("Front"));
		rear.initTable(table.getSubTable("Rear"));
		// tilt.initTable(table.getSubTable("Tilt"));
		feeder.initTable(table.getSubTable("Feeder"));
		upperLimit.initTable(table.getSubTable("Upper"));
		lowerLimit.initTable(table.getSubTable("Lower"));
		// tiltPot.initTable(table.getSubTable("Pot"));
	}

	void turnOnMotors() {
		front.set(onSpeed.get());
		rear.set(onSpeed.get());
	}

	void turnOffMotors() {
		front.set(offSpeed.get());
		rear.set(offSpeed.get());
	}

	void push() {
		feeder.set(feederFinalPosition.get());
	}

	void pull() {
		feeder.set(feederInitialPosition.get());
	}

	public void periodicTeleop() {
		if (control.getLBButton()) {
			turnOnMotors();
		} else {
			turnOffMotors();
		}
		if (control.getRBButton()) {
			push();
		} else {
			pull();

		}

		front.updateTable();
		rear.updateTable();
		// tilt.updateTable();
		feeder.updateTable();
		upperLimit.updateTable();
		lowerLimit.updateTable();
		// tiltPot.updateTable();
	}

	public String getName() {
		return "Shooter";
	}

	public void enableAuto() {
		turnOnMotors();
		sleep(3 * SECOND);
		for (int i = 0; i < 3; i++) {
			push();
			sleep(SECOND);
			pull();
			sleep(SECOND);
		}
		turnOffMotors();
		/*
		Variable motorVariable = new MotorVariable(0.0);
		Variable feederVariable = new FeederVariable(0.0);
		PeriodicTask doPush = new Begin();
		doPush
			.thenSetTo(feederVariable, feederFinalPosition)
			.thenWait(SECOND)
			.thenSetTo(feederVariable, feederInitialPosition);
		PeriodicTask initial = new Begin();
		initial
			.thenSetTo(motorVariable, onSpeed)
			.thenWait(3 * SECOND)
			.thenTimes(3, doPush, new WaitTask(SECOND))
			.thenSetTo(motorVariable, offSpeed);
		
		autoWorker = new TaskWorker(initial);
		autoWorker.start();
		*/
	}

	public void periodicAuto() {
		//autoWorker.step();

	}

	void sleep(long ms) {
		try {
			Thread.sleep(ms);
		} catch (InterruptedException ex) {
			// IGNORE
		}
	}

	class MotorVariable extends SimpleVariable {
		public MotorVariable(double initial) {
			super(initial);
		}
		public void set(double speed) {
			super.set(speed);
			front.set(speed);
			rear.set(speed);
		}
	}
	
	class FeederVariable extends SimpleVariable {
		public FeederVariable(double initial) {
			super(initial);
		}
		public void set(double speed) {
			super.set(speed);
			feeder.set(speed);
		}
	}

}
